#include <PRM.h>
#include <Dijkstra.h>
using namespace M_M_Map;
int main()
{

    Mitrix_M_Map m(15, 15);
    m.addBlock({2, 6});
    m.addBlock({3, 6});
    m.addBlock({4, 6});
    // m.addBlock({5, 6});
    // m.addBlock({6,4});
    m.addBlock({7, 6});
    m.addBlock(10,7);
    m.addBlock(10,8);
    m.addBlock(10,9);
     m.addBlock(10,4);
    m.addBlock(10,5);
    m.addBlock(10,6);
    m_position start={2,1};
    m_position end={8,12};
    m.setStart(start);
    m.setEnd(end);
    m.print();
    PRM prm(&m);
resample:
    prm.SamplePoints(10);
    prm.random_points.insert(prm.random_points.begin(),start);
    prm.random_points.insert(prm.random_points.end(),end);
    M_M_Map::M_Map edge_M_Map(prm.random_points.size());
    for (int i = 0; i < prm.random_points.size(); i++)
    {
        for (int j = 0; j < prm.random_points.size(); j++)
        {
            if (i == j)
            {
                continue;
            }
            Mitrix_StraightPath sp(&m);
            sp.set_S_E(prm.random_points.at(i), prm.random_points.at(j));
            if (sp.expolring())
            {
                edge_M_Map.addEdge({i+1, j+1, 1.0 * sp.pathLength()});
            }
        }
    }
    double distance=Dijkstra(0,prm.random_points.size()-1,edge_M_Map.getDistanceM_Map(),prm.random_points.size());
    if(distance>=M_M_Map_INF-10){
        prm.random_points.clear();
        goto resample;
    }
    else{
        trajectory_node_t* tn=Dijkstra_Path(0,prm.random_points.size()-1,edge_M_Map.getDistanceM_Map(),prm.random_points.size());
        while(tn->next!=NULL){
            Mitrix_StraightPath sp(&m);
            sp.set_S_E(prm.random_points.at(tn->node_id), prm.random_points.at(tn->next->node_id));
            sp.expolring();
            sp.mark();
            m.setTrajectory(prm.random_points.at(tn->next->node_id));
            tn=tn->next;
        }
        for(int i=1;i<prm.random_points.size();i++){
            m.setTrajectory(prm.random_points.at(i));
        }
        cout << endl;
        m.setEnd(end);
        m.print();
    }
}